﻿using System.Diagnostics;
using System.Diagnostics.CodeAnalysis;

namespace SmallGeo;

/// <summary>
/// 二维面内的点或向量
/// </summary>
[DebuggerDisplay("[{X},{Y}]")]
public struct Vector2d
{
    private double x;
    private double y;
    public double X
    {
        readonly get => x;

        set
        {
            x = value;
            ClearExtend();
        }
    }
    public double Y
    {
        readonly get => y;

        set
        {
            y = value;
            ClearExtend();
        }
    }

    [DebuggerStepThrough]
    public Vector2d(double x, double y)
    {
        this.X = x;
        this.Y = y;
    }

    #region 扩展方法属性
    private void ClearExtend()
    {
        _slope = _length = _lengthSquared = null;
    }
    private double? _slope;
    /// <summary>
    /// 斜率, 值域: (-PI,PI]
    /// </summary>
    public double Slope()
    {
        _slope ??= Math.Atan2(Y, X);
        return _slope.Value;
    }

    private double? _length;
    private double? _lengthSquared;
    /// <summary>
    /// 长度
    /// </summary>
    public double Length()
    {
        _length ??= Util.Length(this);
        return _length.Value;
    }

    /// <summary>
    /// 向量长度的平方
    /// </summary>
    public double LengthSquared()
    {
        _lengthSquared ??= Util.LengthSquared(this);
        return _lengthSquared.Value;
    }
    #endregion

    [DebuggerStepThrough]
    public override bool Equals(object? obj)
    {
        if (obj is Vector2d vector) return this == vector;
        return false;
    }

    [DebuggerStepThrough]
    public override int GetHashCode()
    {
        return HashCode.Combine(X, Y);
    }

    [DebuggerStepThrough]
    public override string ToString()
    {
        return $"{{\"X\":{X},\"Y\":{Y}}}";
    }

    public static Vector2d Zero => new Vector2d();
    public static Vector2d UnitX => new Vector2d { X = 1, Y = 0 };
    public static Vector2d UnitY => new Vector2d { X = 0, Y = 1 };

    [DebuggerStepThrough]
    public static bool operator ==(Vector2d a, Vector2d b) => a.X == b.X && a.Y == b.Y;
    [DebuggerStepThrough]
    public static bool operator !=(Vector2d a, Vector2d b) => a.X != b.X || a.Y != b.Y;
    [DebuggerStepThrough]
    public static Vector2d operator +(Vector2d a, Vector2d b) => new Vector2d { X = a.X + b.X, Y = a.Y + b.Y };
    [DebuggerStepThrough]
    public static Vector2d operator -(Vector2d a, Vector2d b) => new Vector2d { X = a.X - b.X, Y = a.Y - b.Y };
    [DebuggerStepThrough]
    public static Vector2d operator *(Vector2d a, Vector2d b) => new Vector2d { X = a.X * b.X, Y = a.Y * b.Y };
    [DebuggerStepThrough]
    public static Vector2d operator *(Vector2d a, double value) => new Vector2d { X = a.X * value, Y = a.Y * value };
    [DebuggerStepThrough]
    public static Vector2d operator *(double value, Vector2d a) => new Vector2d { X = a.X * value, Y = a.Y * value };
    [DebuggerStepThrough]
    public static Vector2d operator /(Vector2d a, double value) => new Vector2d { X = a.X / value, Y = a.Y / value };
    [DebuggerStepThrough]
    public static Vector2d operator -(Vector2d value) => new Vector2d { X = -value.X, Y = -value.Y };
    [DebuggerStepThrough]
    public static bool operator <(Vector2d a, Vector2d b) => a.X < b.X || ((a.X == b.X) && a.Y < b.Y);
    [DebuggerStepThrough]
    public static bool operator >(Vector2d a, Vector2d b) => a.X > b.X || ((a.X == b.X) && a.Y > b.Y);
    [DebuggerStepThrough]
    public static bool operator >=(Vector2d a, Vector2d b) => a.X > b.X || ((a.X == b.X) && a.Y >= b.Y);
    [DebuggerStepThrough]
    public static bool operator <=(Vector2d a, Vector2d b) => a.X < b.X || ((a.X == b.X) && a.Y <= b.Y);
}

/// <summary>
/// 三维面内的点或向量
/// </summary>
[DebuggerDisplay("[{X},{Y},{Z}]")]
public struct Vector3d
{
    public double X { get; set; }
    public double Y { get; set; }
    public double Z { get; set; }

    [DebuggerStepThrough]
    public Vector3d(double x, double y, double z)
    {
        this.X = x;
        this.Y = y;
        this.Z = z;
    }

    [DebuggerStepThrough]
    public override bool Equals(object? obj)
    {
        if (obj is Vector3d vector) return this == vector;
        return false;
    }

    [DebuggerStepThrough]
    public override int GetHashCode()
    {
        return HashCode.Combine(X, Y);
    }

    [DebuggerStepThrough]
    public override string ToString()
    {
        return $"{{\"X\":{X},\"Y\":{Y},\"Z\":{Z}}}";
    }

    public static Vector3d Zero => new Vector3d();
    public static Vector3d UnitX => new Vector3d { X = 1, Y = 0, Z = 0 };
    public static Vector3d UnitY => new Vector3d { X = 0, Y = 1, Z = 0 };
    public static Vector3d UnitZ => new Vector3d { X = 0, Y = 0, Z = 1 };

    [DebuggerStepThrough]
    public static bool operator ==(Vector3d a, Vector3d b) => a.X == b.X && a.Y == b.Y && a.Z == b.Z;
    [DebuggerStepThrough]
    public static bool operator !=(Vector3d a, Vector3d b) => a.X != b.X || a.Y != b.Y && a.Z != b.Z;
    [DebuggerStepThrough]
    public static Vector3d operator +(Vector3d a, Vector3d b) => new Vector3d { X = a.X + b.X, Y = a.Y + b.Y, Z = a.Z + b.Z };
    [DebuggerStepThrough]
    public static Vector3d operator -(Vector3d a, Vector3d b) => new Vector3d { X = a.X - b.X, Y = a.Y - b.Y, Z = a.Z - b.Z };
    [DebuggerStepThrough]
    public static Vector3d operator *(Vector3d a, Vector3d b) => new Vector3d { X = a.X * b.X, Y = a.Y * b.Y, Z = a.Z * b.Z };
    [DebuggerStepThrough]
    public static Vector3d operator *(Vector3d a, double value) => new Vector3d { X = a.X * value, Y = a.Y * value, Z = a.Z * value };
    [DebuggerStepThrough]
    public static Vector3d operator *(double value, Vector3d a) => new Vector3d { X = a.X * value, Y = a.Y * value, Z = a.Z * value };
    [DebuggerStepThrough]
    public static Vector3d operator /(Vector3d a, double value) => new Vector3d { X = a.X / value, Y = a.Y / value, Z = a.Z / value };
    [DebuggerStepThrough]
    public static Vector3d operator -(Vector3d value) => new Vector3d { X = -value.X, Y = -value.Y, Z = -value.Z };
    [DebuggerStepThrough]
    public static bool operator <(Vector3d a, Vector3d b) => a.X < b.X || ((a.X == b.X) && a.Y < b.Y) || ((a.X == b.X) && a.Y == b.Y && a.Z < b.Z);
    [DebuggerStepThrough]
    public static bool operator >(Vector3d a, Vector3d b) => a.X > b.X || ((a.X == b.X) && a.Y > b.Y) || ((a.X == b.X) && (a.Y == b.Y) && a.Z > b.Z);
    [DebuggerStepThrough]
    public static bool operator >=(Vector3d a, Vector3d b) => a.X > b.X || ((a.X == b.X) && a.Y >= b.Y) || ((a.X == b.X) && (a.Y >= b.Y) && a.Z >= b.Z);
    [DebuggerStepThrough]
    public static bool operator <=(Vector3d a, Vector3d b) => a.X < b.X || ((a.X == b.X) && a.Y <= b.Y) || ((a.X == b.X) && (a.Y == b.Y) && a.Z <= b.Z);
}

/// <summary>
/// 二维面内的线段
/// </summary>
[DebuggerDisplay("Name={Name}, [[{P0.X},{P0.Y}],[{P1.X},{P1.Y}]]")]
public class LineSegment2d
{
    [DebuggerStepThrough]
    public LineSegment2d() { }
    [DebuggerStepThrough]
    public LineSegment2d(Vector2d p0, Vector2d p1)
    {
        this.P0 = p0;
        this.P1 = p1;
    }
    public string Name { get; set; }
    public LineSegment2d(Vector2d p0, Vector2d p1, string name) : this(p0, p1)
    {
        this.Name = name;
    }
    private Vector2d p0;
    private Vector2d p1;
    public Vector2d P0
    {
        get => p0;
        set
        {
            p0 = value;
            ClearExtend();
        }
    }
    public Vector2d P1
    {
        get => p1;
        set
        {
            p1 = value;
            ClearExtend();
        }
    }
    [DebuggerStepThrough]
    public LineSegment2d Copy()
    {
        return new LineSegment2d
        {
            P0 = P0,
            P1 = P1,
        };
    }

    #region 扩展方法属性
    private void ClearExtend()
    {
        _slope = _inverseSlope = _length = null;
        _direction = _norm = null;
    }

    private Vector2d? _center;
    /// <summary>
    /// 计算中心点(有缓存, 不会每次都计算)
    /// </summary>
    public Vector2d Center()
    {
        _center ??= (P1 + P0) / 2;
        return _center.Value;
    }

    private double? _length;
    /// <summary>
    /// 计算长度(有缓存, 不会每次都计算)
    /// </summary>
    public double Length()
    {
        _length ??= (P1 - P0).Length();
        return _length.Value;
    }
    private double? _slope;
    /// <summary>
    /// 计算斜率(有缓存, 不会每次都计算)
    /// </summary>
    public double Slope()
    {
        _slope ??= (P1 - P0).Slope();
        return _slope.Value;
    }
    private double? _inverseSlope;
    /// <summary>
    /// 计算反斜率, 即: 从 P1->P0(有缓存, 不会每次都计算)
    /// </summary>
    public double InverseSlope()
    {
        _inverseSlope ??= (P0 - P1).Slope();
        return _inverseSlope.Value;
    }
    private Vector2d? _direction;
    /// <summary>
    /// 计算方向, 已经归一化(有缓存, 不会每次都计算)
    /// </summary>
    public Vector2d Direction()
    {
        _direction ??= (P1 - P0).Normalized();
        return _direction.Value;
    }
    private Vector2d? _norm;
    /// <summary>
    /// 计算线段法向向量, 已经归一化, 逆时针为正, 示意:
    /// <code>
    /// norm
    /// ^
    /// |
    /// |
    /// |
    /// p0------------>p1
    /// </code>
    /// </summary>
    public Vector2d Normal()
    {
        _norm ??= new Vector2d(P0.Y - P1.Y, P1.X - P0.X).Normalized();
        return _norm.Value;
    }
    #endregion

    /// <summary>
    /// 根据给定宽度构造一个矩形, 如下:
    /// <code>
    /// 
    ///    p3----------------p2
    ///    |                 |
    ///    |                 |
    /// line_p0------------line_p1
    ///    |                 |
    ///    |                 |
    ///    p0---------------p1
    /// 
    /// </code>
    /// </summary>
    public List<Vector2d> GetRect(double width)
    {
        var normal = Normal();
        return [P0 - normal * width / 2, P1 - normal * width / 2, P1 + normal * width / 2, P0 + normal * width / 2];
    }
    /// <summary>
    /// 返回垂直的线段, 就是将线绕中点逆时针旋转90°
    /// </summary>
    public LineSegment2d PerpendicularLine()
    {
        return Util.Rotate(this.Copy(), Math.PI / 2, this.Center());
    }
    /// <summary>
    /// 调转此线段方向, 返回自身
    /// </summary>
    public LineSegment2d Inverse()
    {
        (P0, P1) = (P1, P0);
        ClearExtend();
        return this;
    }
    /// <summary>
    /// 返回两个点的新集合
    /// </summary>
    public List<Vector2d> Points() => [P0, P1];

    /// <summary>
    /// 将自身绕中点旋转, 逆时针为正, 弧度制
    /// </summary>
    public LineSegment2d Rotate(double radian)
    {
        (P0, P1) = (Util.RotateRadian(P0, Center(), radian), Util.RotateRadian(P1, Center(), radian));
        ClearExtend();
        return this;
    }

    /// <summary>
    /// 自身移动某个向量, 就是 P0、P1 都+=vector
    /// </summary>
    public LineSegment2d Move(Vector2d vector)
    {
        (P0, P1) = (P0 + vector, P1 + vector);
        ClearExtend();
        return this;
    }

    [DebuggerStepThrough]
    public override string ToString()
    {
        return $"[{{\"X\":{P0.X},\"Y\":{P0.Y}}},{{\"X\":{P1.X},\"Y\":{P1.Y}}}]";
    }

    /// <summary>
    /// 将自身点位四舍五入, 返回自身
    /// </summary>
    public LineSegment2d Round(int digits)
    {
        (P0, P1) = (P0.Round(digits), P1.Round(digits));
        ClearExtend();
        return this;
    }

    /// <summary>
    /// 线段收缩或延长 length 距离, 返回自身
    /// </summary>
    public LineSegment2d Extend(double length)
    {
        (P0, P1) = (P0 - Direction() * length, P1 + Direction() * length);
        ClearExtend();
        return this;
    }
}

public class LineSegment2dEqualityComparer : IEqualityComparer<LineSegment2d>
{
    private readonly double tolerance;
    private readonly bool ignoreDirection;

    public LineSegment2dEqualityComparer(bool ignoreDirection = true, double tolerance = 0.0001)
    {
        this.tolerance = tolerance;
        this.ignoreDirection = ignoreDirection;
    }
    public bool Equals(LineSegment2d? x, LineSegment2d? y)
    {
        if (x == null || y == null) return false;
        if (x.P0.IsNearlySame(y.P0, tolerance) && x.P1.IsNearlySame(y.P1, tolerance)) return true;
        if (ignoreDirection && x.P0.IsNearlySame(y.P1, tolerance) && x.P1.IsNearlySame(y.P0, tolerance)) return true;
        return false;
    }

    public int GetHashCode([DisallowNull] LineSegment2d obj)
    {
        if (tolerance > 0)
        {
            return HashCode.Combine(Math.Round(obj.P0.X / tolerance, 0), Math.Round(obj.P0.Y / tolerance, 0), Math.Round(obj.P1.X / tolerance, 0), Math.Round(obj.P1.Y / tolerance, 0));
        }
        else
        {
            return HashCode.Combine(obj.P0, obj.P1);
        }
    }
}

/// <summary>
/// 三维面内的线段
/// </summary>
[DebuggerDisplay("[[{P0.X},{P0.Y}],[{P1.X},{P1.Y}]]")]
public class LineSegment3d
{
    [DebuggerStepThrough]
    public LineSegment3d() { }
    [DebuggerStepThrough]
    public LineSegment3d(Vector3d p0, Vector3d p1)
    {
        this.P0 = p0;
        this.P1 = p1;
    }
    public Vector3d P0 { get; set; }
    public Vector3d P1 { get; set; }
    [DebuggerStepThrough]
    public LineSegment3d Copy()
    {
        return new LineSegment3d
        {
            P0 = P0,
            P1 = P1,
        };
    }

    [DebuggerStepThrough]
    public override string ToString()
    {
        return $"[{{\"X\":{P0.X},\"Y\":{P0.Y},\"Z\":{P0.Z}}},{{\"X\":{P1.X},\"Y\":{P1.Y},\"Z\":{P1.Z}}}]";
    }
}

/// <summary>
/// 3*3矩阵
/// </summary>
[DebuggerDisplay("[{a11},{a12},{a13},{a21},{a22},{a23},{a31},{a32},{a33}]")]
public struct Matrix_3x3
{
    public double a11 { get; set; }
    public double a12 { get; set; }
    public double a13 { get; set; }
    public double a21 { get; set; }
    public double a22 { get; set; }
    public double a23 { get; set; }
    public double a31 { get; set; }
    public double a32 { get; set; }
    public double a33 { get; set; }

    public double[] Elements => [a11, a12, a13, a21, a22, a23, a31, a32, a33];

    [DebuggerStepThrough]
    public Matrix_3x3 SetElements(double[] elements)
    {
        if (elements == null || elements.Length != 9)
            throw new Exception($"不能给 Matrix_3x3 设置空的或数量不等于9的数组!");
        a11 = elements[0]; a12 = elements[1]; a13 = elements[2];
        a21 = elements[3]; a22 = elements[4]; a23 = elements[5];
        a31 = elements[6]; a32 = elements[7]; a33 = elements[8];

        return this;
    }
    /// <summary>
    /// 特征值
    /// </summary>
    /// <returns></returns>
    [DebuggerStepThrough]
    public double Determinant()
        => (a11 * a22 * a33 + a12 * a23 * a31 + a13 * a21 * a32) - (a11 * a23 * a32 + a12 * a21 * a33 + a13 * a22 * a31);

    public static Matrix_3x3 Zero => new Matrix_3x3();
    public static Matrix_3x3 One => new Matrix_3x3().SetElements([
        1, 0, 0,
        0, 1, 0,
        0, 0, 1
    ]);

    /// <summary>
    /// 应用线性变换
    /// </summary>
    /// <param name="point"></param>
    /// <returns></returns>
    [DebuggerStepThrough]
    public Vector2d Apply(Vector2d point)
    {
        return new Vector2d
        {
            X = this.a11 * point.X + this.a12 * point.Y + this.a13,
            Y = this.a21 * point.X + this.a22 * point.Y + this.a23,
        };
    }

    /// <summary>
    /// 返回新的逆矩阵
    /// </summary>
    /// <returns></returns>
    [DebuggerStepThrough]
    public Matrix_3x3 Invert()
    {
        double t11 = a33 * a22 - a32 * a23;
        double t12 = a32 * a13 - a33 * a12;
        double t13 = a23 * a12 - a22 * a13;
        double det = a11 * t11 + a21 * t12 + a31 * t13;

        //没有逆矩阵
        if (det == 0) return Matrix_3x3.Zero;

        var detInv = 1 / det;
        double
            _a11, _a12, _a13,
            _a21, _a22, _a23,
            _a31, _a32, _a33;

        _a11 = t11 * detInv;
        _a12 = (a31 * a23 - a33 * a21) * detInv;
        _a13 = (a32 * a21 - a31 * a22) * detInv;

        _a21 = t12 * detInv;
        _a22 = (a33 * a11 - a31 * a13) * detInv;
        _a23 = (a31 * a12 - a32 * a11) * detInv;

        _a31 = t13 * detInv;
        _a32 = (a21 * a13 - a23 * a11) * detInv;
        _a33 = (a22 * a11 - a21 * a12) * detInv;

        return new Matrix_3x3().SetElements(new double[]
        {
            _a11,_a12,_a13,
            _a21,_a22,_a23,
            _a31,_a32,_a33,
        });
    }

    [DebuggerStepThrough]
    public Matrix_3x3 Copy()
    {
        return new Matrix_3x3().SetElements(new[]
        {
            a11,a12,a13,
            a21,a22,a23,
            a31,a32,a33,
        });
    }

    /// <summary>
    /// 矩阵相乘,返回新的矩阵
    /// </summary>
    [DebuggerStepThrough]
    public static Matrix_3x3 operator *(Matrix_3x3 a, Matrix_3x3 b)
    {
        var ae = a.Elements;
        var be = b.Elements;
        var te = new double[9];

        double a11 = ae[0], a12 = ae[3], a13 = ae[6];
        double a21 = ae[1], a22 = ae[4], a23 = ae[7];
        double a31 = ae[2], a32 = ae[5], a33 = ae[8];

        double b11 = be[0], b12 = be[3], b13 = be[6];
        double b21 = be[1], b22 = be[4], b23 = be[7];
        double b31 = be[2], b32 = be[5], b33 = be[8];

        te[0] = a11 * b11 + a12 * b21 + a13 * b31;
        te[3] = a11 * b12 + a12 * b22 + a13 * b32;
        te[6] = a11 * b13 + a12 * b23 + a13 * b33;

        te[1] = a21 * b11 + a22 * b21 + a23 * b31;
        te[4] = a21 * b12 + a22 * b22 + a23 * b32;
        te[7] = a21 * b13 + a22 * b23 + a23 * b33;

        te[2] = a31 * b11 + a32 * b21 + a33 * b31;
        te[5] = a31 * b12 + a32 * b22 + a33 * b32;
        te[8] = a31 * b13 + a32 * b23 + a33 * b33;

        return new Matrix_3x3().SetElements(te);
    }

    /// <summary>
    /// 乘以标量,返回新矩阵
    /// </summary>
    [DebuggerStepThrough]
    public Matrix_3x3 MultiplyScalar(double s)
        => new Matrix_3x3().SetElements(new[]
        {
            a11*s,a12*s,a13*s,
            a21*s,a22*s,a23*s,
            a31*s,a32*s,a33*s,
        });

    /// <summary>
    /// 矩阵转置,返回新矩阵
    /// </summary>
    [DebuggerStepThrough]
    public Matrix_3x3 Transpose()
        => new Matrix_3x3().SetElements(new[]
        {
            a11, a21, a31,
            a12, a22, a32,
            a13, a23, a33,
        });

    /// <summary>
    /// 从缩放中创建矩阵
    /// </summary>
    [DebuggerStepThrough]
    public static Matrix_3x3 FromScale(double x, double y)
        => new Matrix_3x3().SetElements(new[]
        {
            x, 0, 0,
            0, y, 0,
            0, 0, 1,
        });

    /// <summary>
    /// 从缩放中创建矩阵,相对指定点
    /// </summary>
    [DebuggerStepThrough]
    public static Matrix_3x3 FromScale(Vector2d origin, double x, double y)
        => Matrix_3x3.FromTranslate(-origin.X, -origin.Y) * Matrix_3x3.FromScale(x, y) * Matrix_3x3.FromTranslate(origin.X, origin.Y);

    /// <summary>
    /// 从平移中创建矩阵
    /// </summary>
    [DebuggerStepThrough]
    public static Matrix_3x3 FromTranslate(double x, double y)
        => new Matrix_3x3().SetElements(new[]
        {
            1, 0, x,
            0, 1, y,
            0, 0, 1,
        });

    /// <summary>
    /// 从旋转中创建矩阵
    /// </summary>
    [DebuggerStepThrough]
    public static Matrix_3x3 FromRotation(double radian)
    {
        double c = Math.Cos(radian);
        double s = Math.Sin(radian);

        return new Matrix_3x3().SetElements(new[]
        {
            c, -s,  0,
            s,  c,  0,
            0,  0,  1,
        });
    }

    /// <summary>
    /// 从旋转中创建矩阵, 绕指定点旋转
    /// </summary>
    [DebuggerStepThrough]
    public static Matrix_3x3 FromRotation(Vector2d origin, double radian)
        => Matrix_3x3.FromTranslate(-origin.X, -origin.Y)
            * Matrix_3x3.FromRotation(radian)
            * Matrix_3x3.FromTranslate(origin.X, origin.Y);

    /// <summary>
    /// 左乘: m * this
    /// 返回新向量
    /// </summary>
    [DebuggerStepThrough]
    public Matrix_3x3 PreMultiply(Matrix_3x3 m)
        => m * this;

    /// <summary>
    /// 右乘: this * m
    /// 返回新向量
    /// </summary>
    [DebuggerStepThrough]
    public Matrix_3x3 Multiply(Matrix_3x3 m)
        => this * m;

    /// <summary>
    /// 再进行一次旋转
    /// </summary>
    [DebuggerStepThrough]
    public Matrix_3x3 ThenRotate(double radian)
        => Matrix_3x3.FromRotation(radian) * this;
    /// <summary>
    /// 再进行一次旋转,绕指定点
    /// </summary>
    [DebuggerStepThrough]
    public Matrix_3x3 ThenRotate(Vector2d origin, double radian)
        => Matrix_3x3.FromRotation(origin, radian) * this;
    /// <summary>
    /// 再进行一次平移
    /// </summary>
    [DebuggerStepThrough]
    public Matrix_3x3 ThenTransform(double x, double y)
        => Matrix_3x3.FromTranslate(x, y) * this;
    /// <summary>
    /// 再进行一次缩放
    /// </summary>
    [DebuggerStepThrough]
    public Matrix_3x3 ThenScale(double x, double y)
        => Matrix_3x3.FromScale(x, y) * this;
    /// <summary>
    /// 再进行一次缩放
    /// </summary>
    [DebuggerStepThrough]
    public Matrix_3x3 ThenScale(Vector2d origin, double x, double y)
        => Matrix_3x3.FromScale(origin, x, y) * this;
}

/// <summary>
/// 4*4矩阵
/// </summary>
[DebuggerDisplay("[{a11},{a12},{a13},{a14},{a21},{a22},{a23},{a24},{a31},{a32},{a33},{a34},{a41},{a42},{a43},{a44}]")]
public struct Matrix_4x4
{
    public double a11 { get; set; }
    public double a12 { get; set; }
    public double a13 { get; set; }
    public double a14 { get; set; }
    public double a21 { get; set; }
    public double a22 { get; set; }
    public double a23 { get; set; }
    public double a24 { get; set; }
    public double a31 { get; set; }
    public double a32 { get; set; }
    public double a33 { get; set; }
    public double a34 { get; set; }
    public double a41 { get; set; }
    public double a42 { get; set; }
    public double a43 { get; set; }
    public double a44 { get; set; }

    public double[] Elements => [
        a11, a12, a13, a14,
        a21, a22, a23, a24,
        a31, a32, a33, a34,
        a41, a42, a43, a44];

    [DebuggerStepThrough]
    public Matrix_4x4 SetElements(double[] elements)
    {
        if (elements == null || elements.Length != 16)
            throw new Exception($"不能给 Matrix_4x4 设置空的或数量不等于16的数组!");
        a11 = elements[0]; a12 = elements[1]; a13 = elements[2]; a14 = elements[3];
        a21 = elements[4]; a22 = elements[5]; a23 = elements[6]; a24 = elements[7];
        a31 = elements[8]; a32 = elements[9]; a33 = elements[10]; a34 = elements[11];
        a41 = elements[12]; a42 = elements[13]; a43 = elements[14]; a44 = elements[15];
        return this;
    }
    /// <summary>
    /// 特征值
    /// </summary>
    [DebuggerStepThrough]
    public double Determinant()
        => a41 * (
            +a14 * a23 * a32
             - a13 * a24 * a32
             - a14 * a22 * a33
             + a12 * a24 * a33
             + a13 * a22 * a34
             - a12 * a23 * a34
        ) +
        a42 * (
            +a11 * a23 * a34
             - a11 * a24 * a33
             + a14 * a21 * a33
             - a13 * a21 * a34
             + a13 * a24 * a31
             - a14 * a23 * a31
        ) +
        a43 * (
            +a11 * a24 * a32
             - a11 * a22 * a34
             - a14 * a21 * a32
             + a12 * a21 * a34
             + a14 * a22 * a31
             - a12 * a24 * a31
        ) +
        a44 * (
            -a13 * a22 * a31
             - a11 * a23 * a32
             + a11 * a22 * a33
             + a13 * a21 * a32
             - a12 * a21 * a33
             + a12 * a23 * a31
        );

    public static Matrix_4x4 Zero => new Matrix_4x4();
    public static Matrix_4x4 One => new Matrix_4x4().SetElements(new double[] {
        1, 0, 0, 0,
        0, 1, 0, 0,
        0, 0, 1, 0,
        0, 0, 0, 1,
    });

    /// <summary>
    /// 应用线性变换
    /// </summary>
    /// <param name="point"></param>
    /// <returns></returns>
    [DebuggerStepThrough]
    public Vector3d Apply(Vector3d point)
    {
        return new Vector3d
        {
            X = this.a11 * point.X + this.a12 * point.Y + this.a13 * point.Z + this.a14,
            Y = this.a21 * point.X + this.a22 * point.Y + this.a23 * point.Z + this.a24,
            Z = this.a31 * point.X + this.a32 * point.Y + this.a33 * point.Z + this.a34,
        };
    }
    /// <summary>
    /// 返回新的逆矩阵
    /// </summary>
    /// <returns></returns>
    [DebuggerStepThrough]
    public Matrix_4x4 Invert()
    {
        // based on http://www.euclideanspace.com/maths/algebra/matrix/functions/inverse/fourD/index.htm
        double[] te = new double[16];
        Array.Copy(this.Elements, te, 16);

        double
            n11 = te[0], n21 = te[1], n31 = te[2], n41 = te[3],
            n12 = te[4], n22 = te[5], n32 = te[6], n42 = te[7],
            n13 = te[8], n23 = te[9], n33 = te[10], n43 = te[11],
            n14 = te[12], n24 = te[13], n34 = te[14], n44 = te[15],

            t11 = n23 * n34 * n42 - n24 * n33 * n42 + n24 * n32 * n43 - n22 * n34 * n43 - n23 * n32 * n44 + n22 * n33 * n44,
            t12 = n14 * n33 * n42 - n13 * n34 * n42 - n14 * n32 * n43 + n12 * n34 * n43 + n13 * n32 * n44 - n12 * n33 * n44,
            t13 = n13 * n24 * n42 - n14 * n23 * n42 + n14 * n22 * n43 - n12 * n24 * n43 - n13 * n22 * n44 + n12 * n23 * n44,
            t14 = n14 * n23 * n32 - n13 * n24 * n32 - n14 * n22 * n33 + n12 * n24 * n33 + n13 * n22 * n34 - n12 * n23 * n34;

        double det = n11 * t11 + n21 * t12 + n31 * t13 + n41 * t14;

        if (det == 0) return Matrix_4x4.Zero;

        double detInv = 1 / det;

        te[0] = t11 * detInv;
        te[1] = (n24 * n33 * n41 - n23 * n34 * n41 - n24 * n31 * n43 + n21 * n34 * n43 + n23 * n31 * n44 - n21 * n33 * n44) * detInv;
        te[2] = (n22 * n34 * n41 - n24 * n32 * n41 + n24 * n31 * n42 - n21 * n34 * n42 - n22 * n31 * n44 + n21 * n32 * n44) * detInv;
        te[3] = (n23 * n32 * n41 - n22 * n33 * n41 - n23 * n31 * n42 + n21 * n33 * n42 + n22 * n31 * n43 - n21 * n32 * n43) * detInv;

        te[4] = t12 * detInv;
        te[5] = (n13 * n34 * n41 - n14 * n33 * n41 + n14 * n31 * n43 - n11 * n34 * n43 - n13 * n31 * n44 + n11 * n33 * n44) * detInv;
        te[6] = (n14 * n32 * n41 - n12 * n34 * n41 - n14 * n31 * n42 + n11 * n34 * n42 + n12 * n31 * n44 - n11 * n32 * n44) * detInv;
        te[7] = (n12 * n33 * n41 - n13 * n32 * n41 + n13 * n31 * n42 - n11 * n33 * n42 - n12 * n31 * n43 + n11 * n32 * n43) * detInv;

        te[8] = t13 * detInv;
        te[9] = (n14 * n23 * n41 - n13 * n24 * n41 - n14 * n21 * n43 + n11 * n24 * n43 + n13 * n21 * n44 - n11 * n23 * n44) * detInv;
        te[10] = (n12 * n24 * n41 - n14 * n22 * n41 + n14 * n21 * n42 - n11 * n24 * n42 - n12 * n21 * n44 + n11 * n22 * n44) * detInv;
        te[11] = (n13 * n22 * n41 - n12 * n23 * n41 - n13 * n21 * n42 + n11 * n23 * n42 + n12 * n21 * n43 - n11 * n22 * n43) * detInv;

        te[12] = t14 * detInv;
        te[13] = (n13 * n24 * n31 - n14 * n23 * n31 + n14 * n21 * n33 - n11 * n24 * n33 - n13 * n21 * n34 + n11 * n23 * n34) * detInv;
        te[14] = (n14 * n22 * n31 - n12 * n24 * n31 - n14 * n21 * n32 + n11 * n24 * n32 + n12 * n21 * n34 - n11 * n22 * n34) * detInv;
        te[15] = (n12 * n23 * n31 - n13 * n22 * n31 + n13 * n21 * n32 - n11 * n23 * n32 - n12 * n21 * n33 + n11 * n22 * n33) * detInv;

        return new Matrix_4x4().SetElements(te);
    }

    [DebuggerStepThrough]
    public Matrix_4x4 Copy()
    {
        return new Matrix_4x4().SetElements(new[]
        {
            a11,a12,a13,a14,
            a21,a22,a23,a24,
            a31,a32,a33,a34,
            a41,a42,a43,a44,
        });
    }

    /// <summary>
    /// 矩阵相乘,返回新的矩阵
    /// </summary>
    [DebuggerStepThrough]
    public static Matrix_4x4 operator *(Matrix_4x4 a, Matrix_4x4 b)
    {
        var ae = a.Elements;
        var be = b.Elements;

        double a11 = ae[0], a12 = ae[4], a13 = ae[8], a14 = ae[12];
        double a21 = ae[1], a22 = ae[5], a23 = ae[9], a24 = ae[13];
        double a31 = ae[2], a32 = ae[6], a33 = ae[10], a34 = ae[14];
        double a41 = ae[3], a42 = ae[7], a43 = ae[11], a44 = ae[15];

        double b11 = be[0], b12 = be[4], b13 = be[8], b14 = be[12];
        double b21 = be[1], b22 = be[5], b23 = be[9], b24 = be[13];
        double b31 = be[2], b32 = be[6], b33 = be[10], b34 = be[14];
        double b41 = be[3], b42 = be[7], b43 = be[11], b44 = be[15];

        double[] te =
        [
            a11 * b11 + a12 * b21 + a13 * b31 + a14 * b41,//11
            a11 * b12 + a12 * b22 + a13 * b32 + a14 * b42,//12
            a11 * b13 + a12 * b23 + a13 * b33 + a14 * b43,//13
            a11 * b14 + a12 * b24 + a13 * b34 + a14 * b44,//14
            a21 * b11 + a22 * b21 + a23 * b31 + a24 * b41,//21
            a21 * b12 + a22 * b22 + a23 * b32 + a24 * b42,//22
            a21 * b13 + a22 * b23 + a23 * b33 + a24 * b43,//23
            a21 * b14 + a22 * b24 + a23 * b34 + a24 * b44,//24
            a31 * b11 + a32 * b21 + a33 * b31 + a34 * b41,//31
            a31 * b12 + a32 * b22 + a33 * b32 + a34 * b42,//32
            a31 * b13 + a32 * b23 + a33 * b33 + a34 * b43,//33
            a31 * b14 + a32 * b24 + a33 * b34 + a34 * b44,//34
            a41 * b11 + a42 * b21 + a43 * b31 + a44 * b41,//41
            a41 * b12 + a42 * b22 + a43 * b32 + a44 * b42,//42
            a41 * b13 + a42 * b23 + a43 * b33 + a44 * b43,//43
            a41 * b14 + a42 * b24 + a43 * b34 + a44 * b44,//44
        ];
        return new Matrix_4x4().SetElements(te);
    }

    /// <summary>
    /// 乘以标量,返回新矩阵
    /// </summary>
    [DebuggerStepThrough]
    public Matrix_4x4 MultiplyScalar(double s)
        => new Matrix_4x4().SetElements(new[]
        {
            a11*s,a12*s,a13*s,a14*s,
            a21*s,a22*s,a23*s,a24*s,
            a31*s,a32*s,a33*s,a34*s,
            a41*s,a42*s,a43*s,a44*s,
        });

    /// <summary>
    /// 矩阵转置,返回新矩阵
    /// </summary>
    [DebuggerStepThrough]
    public Matrix_4x4 Transpose()
        => new Matrix_4x4().SetElements(new[]
        {
            a11, a21, a31, a41,
            a12, a22, a32, a42,
            a13, a23, a33, a43,
            a14, a24, a34, a44,
        });

    /// <summary>
    /// 从缩放中创建矩阵
    /// </summary>
    [DebuggerStepThrough]
    public static Matrix_4x4 FromScale(double x, double y, double z)
        => new Matrix_4x4().SetElements(new[]
        {
            x, 0, 0, 0,
            0, y, 0, 0,
            0, 0, z, 0,
            0, 0, 0, 1,
        });

    /// <summary>
    /// 从缩放中创建矩阵
    /// </summary>
    [DebuggerStepThrough]
    public static Matrix_4x4 FromScale(Vector3d origin, double x, double y, double z)
        => Matrix_4x4.FromTranslate(-origin.X, -origin.Y, -origin.Z) * Matrix_4x4.FromScale(x, y, z) * Matrix_4x4.FromTranslate(origin.X, origin.Y, origin.Z);

    /// <summary>
    /// 从平移中创建矩阵
    /// </summary>
    [DebuggerStepThrough]
    public static Matrix_4x4 FromTranslate(double x, double y, double z)
        => new Matrix_4x4().SetElements(new[]
        {
            1, 0, 0, x,
            0, 1, 0, y,
            0, 0, 1, z,
            0, 0, 0, 1,
        });

    /// <summary>
    /// 从旋转中创建矩阵,绕X轴
    /// </summary>
    [DebuggerStepThrough]
    public static Matrix_4x4 FromRotationX(double theta)
    {
        double c = Math.Cos(theta);
        double s = Math.Sin(theta);

        return new Matrix_4x4().SetElements(new[]
        {
            1, 0,  0, 0,
            0, c, -s, 0,
            0, s,  c, 0,
            0, 0,  0, 1
        });
    }

    /// <summary>
    /// 从旋转中创建矩阵,绕Y轴
    /// </summary>
    [DebuggerStepThrough]
    public static Matrix_4x4 FromRotationY(double theta)
    {
        double c = Math.Cos(theta);
        double s = Math.Sin(theta);

        return new Matrix_4x4().SetElements(new[]
        {
             c, 0, s, 0,
             0, 1, 0, 0,
            -s, 0, c, 0,
             0, 0, 0, 1
        });
    }

    /// <summary>
    /// 从旋转中创建矩阵,绕Z轴
    /// </summary>
    [DebuggerStepThrough]
    public static Matrix_4x4 FromRotationZ(double theta)
    {
        double c = Math.Cos(theta);
        double s = Math.Sin(theta);

        return new Matrix_4x4().SetElements(new[]
        {
            c, -s, 0, 0,
            s,  c, 0, 0,
            0,  0, 1, 0,
            0,  0, 0, 1
        });
    }

    /// <summary>
    /// 从旋转中创建矩阵,绕X轴,过指定点
    /// </summary>
    [DebuggerStepThrough]
    public static Matrix_4x4 FromRotationX(Vector3d throughPoint, double theta)
        => Matrix_4x4.FromTranslate(-throughPoint.X, -throughPoint.Y, -throughPoint.Z) * Matrix_4x4.FromRotationX(theta) * Matrix_4x4.FromTranslate(throughPoint.X, throughPoint.Y, throughPoint.Z);

    /// <summary>
    /// 从旋转中创建矩阵,绕Y轴
    /// </summary>
    [DebuggerStepThrough]
    public static Matrix_4x4 FromRotationY(Vector3d throughPoint, double theta)
        => Matrix_4x4.FromTranslate(-throughPoint.X, -throughPoint.Y, -throughPoint.Z) * Matrix_4x4.FromRotationY(theta) * Matrix_4x4.FromTranslate(throughPoint.X, throughPoint.Y, throughPoint.Z);

    /// <summary>
    /// 从旋转中创建矩阵,绕Z轴
    /// </summary>
    [DebuggerStepThrough]
    public static Matrix_4x4 FromRotationZ(Vector3d throughPoint, double theta)
        => Matrix_4x4.FromTranslate(-throughPoint.X, -throughPoint.Y, -throughPoint.Z) * Matrix_4x4.FromRotationZ(theta) * Matrix_4x4.FromTranslate(throughPoint.X, throughPoint.Y, throughPoint.Z);

    /// <summary>
    /// 绕任意轴旋转, 右手坐标系: 大拇指指向轴方向, 其他手指弯曲方向为旋转正方向
    /// </summary>
    [DebuggerStepThrough]
    public static Matrix_4x4 FromRotationAxis(Vector3d axis, double radian)
    {

        // Based on http://www.gamedev.net/reference/articles/article1199.asp

        double c = Math.Cos(radian);
        double s = Math.Sin(radian);
        double t = 1 - c;
        double x = axis.X, y = axis.Y, z = axis.Z;
        double tx = t * x, ty = t * y;

        return new Matrix_4x4().SetElements(new[]
        {
            tx * x + c,         tx * y - s * z,     tx * z + s * y,         0,
            tx * y + s * z,     ty * y + c,         ty * z - s * x,         0,
            tx * z - s * y,     ty * z + s * x,     t * z * z + c,          0,
            0,                  0,                  0,                      1
        });
    }

    /// <summary>
    /// 绕任意轴旋转, 右手坐标系: 大拇指指向轴方向, 其他手指弯曲方向为旋转正方向
    /// </summary>
    [DebuggerStepThrough]
    public static Matrix_4x4 FromRotationAxis(Vector3d throughPoint, Vector3d axis, double theta)
        => Matrix_4x4.FromTranslate(-throughPoint.X, -throughPoint.Y, -throughPoint.Z) * Matrix_4x4.FromRotationAxis(axis, theta) * Matrix_4x4.FromTranslate(throughPoint.X, throughPoint.Y, throughPoint.Z);

    /// <summary>
    /// 左乘: m * this
    /// 返回新向量
    /// </summary>
    [DebuggerStepThrough]
    public Matrix_4x4 PreMultiply(Matrix_4x4 m)
        => m * this;

    /// <summary>
    /// 右乘: this * m
    /// 返回新向量
    /// </summary>
    [DebuggerStepThrough]
    public Matrix_4x4 Multiply(Matrix_4x4 m)
        => this * m;

    /// <summary>
    /// 再进行一次旋转
    /// </summary>
    [DebuggerStepThrough]
    public Matrix_4x4 ThenRotateX(double radian)
        => Matrix_4x4.FromRotationX(radian) * this;

    /// <summary>
    /// 再进行一次旋转
    /// </summary>
    [DebuggerStepThrough]
    public Matrix_4x4 ThenRotateX(Vector3d throughPoint, double radian)
        => Matrix_4x4.FromRotationX(throughPoint, radian) * this;
    /// <summary>
    /// 再进行一次旋转
    /// </summary>
    [DebuggerStepThrough]
    public Matrix_4x4 ThenRotateY(double radian)
        => Matrix_4x4.FromRotationY(radian) * this;
    /// <summary>
    /// 再进行一次旋转
    /// </summary>
    [DebuggerStepThrough]
    public Matrix_4x4 ThenRotateY(Vector3d throughPoint, double radian)
        => Matrix_4x4.FromRotationY(throughPoint, radian) * this;
    /// <summary>
    /// 再进行一次旋转
    /// </summary>
    [DebuggerStepThrough]
    public Matrix_4x4 ThenRotateZ(double radian)
        => Matrix_4x4.FromRotationZ(radian) * this;
    /// <summary>
    /// 再进行一次旋转
    /// </summary>
    [DebuggerStepThrough]
    public Matrix_4x4 ThenRotateZ(Vector3d throughPoint, double radian)
        => Matrix_4x4.FromRotationZ(throughPoint, radian) * this;
    /// <summary>
    /// 再进行一次旋转,绕指定点
    /// </summary>
    [DebuggerStepThrough]
    public Matrix_4x4 ThenRotateAxis(Vector3d axis, double radian)
        => Matrix_4x4.FromRotationAxis(axis, radian) * this;
    /// <summary>
    /// 再进行一次旋转,绕指定点
    /// </summary>
    [DebuggerStepThrough]
    public Matrix_4x4 ThenRotateAxis(Vector3d throughPoint, Vector3d axis, double radian)
        => Matrix_4x4.FromRotationAxis(throughPoint, axis, radian) * this;
    /// <summary>
    /// 再进行一次平移
    /// </summary>
    [DebuggerStepThrough]
    public Matrix_4x4 ThenTransform(double x, double y, double z)
        => Matrix_4x4.FromTranslate(x, y, z) * this;
    /// <summary>
    /// 再进行一次缩放
    /// </summary>
    [DebuggerStepThrough]
    public Matrix_4x4 ThenScale(double x, double y, double z)
        => Matrix_4x4.FromScale(x, y, z) * this;
    /// <summary>
    /// 再进行一次缩放
    /// </summary>
    [DebuggerStepThrough]
    public Matrix_4x4 ThenScale(Vector3d origin, double x, double y, double z)
        => Matrix_4x4.FromScale(origin, x, y, z) * this;
}

/// <summary>
/// 射线
/// </summary>
[DebuggerDisplay("Origin=[{Origin.X},{Origin.Y}], Direction=[{Direction.X},{Direction.Y}]")]
public class Ray2d
{
    [DebuggerStepThrough]
    public Ray2d(Vector2d origin, Vector2d direction)
    {
        this.Direction = direction;
        this.Origin = origin;
    }
    [DebuggerStepThrough]
    public Ray2d() { }
    public Vector2d Origin { get; set; }
    public Vector2d Direction { get; set; }

    #region 方法
    /// <summary>
    /// 延射线方向移动一段距离
    /// </summary>
    /// <param name="lenth"></param>
    /// <returns></returns>
    [DebuggerStepThrough]
    public Ray2d Move(double lenth)
    {
        this.Origin += lenth * this.Direction;
        return this;
    }
    #endregion
}
/// <summary>
/// 射线
/// </summary>
[DebuggerDisplay("Origin=[{Origin.X},{Origin.Y},{Origin.Z}], Direction=[{Direction.X},{Direction.Y},{Direction.Z}]")]
public class Ray3d
{
    [DebuggerStepThrough]
    public Ray3d(Vector3d origin, Vector3d direction)
    {
        this.Direction = direction;
        this.Origin = origin;
    }
    [DebuggerStepThrough]
    public Ray3d() { }
    public Vector3d Origin { get; set; }
    public Vector3d Direction { get; set; }

    #region 方法
    /// <summary>
    /// 延射线方向移动一段距离
    /// </summary>
    /// <param name="lenth"></param>
    /// <returns></returns>
    [DebuggerStepThrough]
    public Ray3d Move(double lenth)
    {
        this.Origin += lenth * this.Direction;
        return this;
    }
    #endregion
}